// snakebot.c
//
// Clark Scheff
//
// 

// Defines for the limits of the servo movements
#define JOINT_MIN	550
#define JOINT_MID	1400
#define JOINT_MAX	2360

// defines for the orientation of the snakebot
// STOMACH_SIDE is lying down on the stomach or starting position
// BACK_SIDE is the opposite side
#define STOMACH_SIDE	0
#define BACK_SIDE	1

// Include our I/O definitions
#include <avr/io.h>

// Include for using delay routines
#include <util/delay.h>

// Include for the analog subsystem
#include "servo.h"

// Include information about the device we're building for
#include "device.h"

// Globals
unsigned char servos[4];
unsigned char orientation = STOMACH_SIDE;

// Function Prototypes
void delayms( uint16_t millis );
void lieFlat(void);
void rollUp(unsigned char);
void cobraStance(void);
void yesNod(void);
void tailWag(void);
void pushUps(void);
void bendOver(void);
void rockClimbers(void);
void flipOver(void);
void inchWorm(unsigned char small);


// function to delay in milliseconds
void delayms( uint16_t millis ) {
	while ( millis ) {
		_delay_ms( 1 );
		millis--;
	}
}

// And now for our main routine:
int main(void)
{
	// Some loop variables
	unsigned char i;
	
	// Initialize the servo system
	servo_init();

	for(i = 0; i < 3; i++)
	{
		// Define our servos to use PC0-PC3
		servos[i] = servo_define(_SFR_IO_ADDR(DDRC), _SFR_IO_ADDR(PORTC), i);

		// Turn the servo on
		servo_active(servos[i]);
	}

	lieFlat();
	delayms(1000);
	
	// Our endless loop
	for(i = 0; i < 4; i++)
	{
		cobraStance();
		delayms(500);
		yesNod();
		tailWag();
		lieFlat();
		bendOver();
		rockClimbers();
		pushUps();
		inchWorm(1);		
		inchWorm(0);
		lieFlat();		
		flipOver();
		delayms(500);
	}
	
	// turn off all the servos and shut down
	for(i = 0; i < 4; i++)
	{
		// Turn the servo off
		servo_inactive(servos[i]);
	}
	
	return 0;
}


void lieFlat(void)
{
	servo_set(servos[0], JOINT_MID);
	servo_set(servos[1], JOINT_MID);
	servo_set(servos[2], JOINT_MID);
	delayms(500);
}

void rollUp(unsigned char up)
{
	if(up)
	{
		if(orientation == STOMACH_SIDE)
		{
			servo_set(servos[0], JOINT_MIN);
			delayms(350);
			servo_set(servos[1], JOINT_MAX);
			servo_set(servos[2], JOINT_MIN);
			delayms(350);
		}
		else
		{
			servo_set(servos[0], JOINT_MAX);
			delayms(350);
			servo_set(servos[1], JOINT_MIN);
			servo_set(servos[2], JOINT_MAX);
			delayms(350);
		}
	}
	else
	{
		if(orientation == STOMACH_SIDE)
		{
			servo_set(servos[0], JOINT_MAX);
			delayms(350);
			servo_set(servos[1], JOINT_MIN);
			servo_set(servos[2], JOINT_MAX);
			delayms(350);
		}
		else
		{
			servo_set(servos[0], JOINT_MIN);
			delayms(350);
			servo_set(servos[1], JOINT_MAX);
			servo_set(servos[2], JOINT_MIN);
			delayms(350);
		}
	}
}

void cobraStance(void)
{
	rollUp(1);
	if(orientation == STOMACH_SIDE)
	{
		servo_set(servos[0], 2000);
		servo_set(servos[2], 800);
		delayms(350);
	}
	else
	{
		servo_set(servos[0], 800);
		servo_set(servos[2], 2000);
		delayms(350);
	}
}
		
void yesNod(void)
{
	unsigned char i;
	
	for(i = 0; i < 4; i++)
	{
		if(orientation == STOMACH_SIDE)
		{
			servo_set(servos[0], 2200);
			delayms(200);
			servo_set(servos[0], 2000);
			delayms(200);
		}
		else
		{
			servo_set(servos[0], 1000);
			delayms(200);
			servo_set(servos[0], 800);
			delayms(200);
		}
	}
}

void tailWag(void)
{
	unsigned char i;
	
	for(i = 0; i < 8; i++)
	{
		if(orientation == STOMACH_SIDE)
		{
			servo_set(servos[2], 1000);
			delayms(100);
			servo_set(servos[2], 800);
			delayms(100);
		}
		else
		{
			servo_set(servos[2], 1800);
			delayms(100);
			servo_set(servos[2], 2000);
			delayms(100);
		}
	}
}

void pushUps(void)
{
	unsigned char i;
	
	lieFlat();
	
	for(i = 0; i < 10; i++)
	{
		if(orientation == STOMACH_SIDE)
		{
			servo_set(servos[0], 1800);
			servo_set(servos[2], 1800);
			delayms(200);
		}
		else
		{
			servo_set(servos[0], 1000);
			servo_set(servos[2], 1000);
			delayms(200);
		}
		
		servo_set(servos[0], JOINT_MID);
		servo_set(servos[2], JOINT_MID);
		delayms(250);
	}
}

void bendOver(void)
{
	if(orientation == STOMACH_SIDE)
	{
		servo_set(servos[0], JOINT_MAX);
		delayms(350);
		servo_set(servos[1], JOINT_MIN);
		servo_set(servos[2], JOINT_MIN);
		delayms(350);
	}
	else
	{
		servo_set(servos[0], JOINT_MIN);
		delayms(350);
		servo_set(servos[1], JOINT_MAX);
		servo_set(servos[2], JOINT_MAX);
		delayms(350);
	}
}

void rockClimbers(void)
{
	unsigned char i;
	
	for(i = 0; i < 5; i++)
	{
		if(orientation == STOMACH_SIDE)
		{
			servo_set(servos[2], 800);
			servo_set(servos[1], 700);
			delayms(250);
			servo_set(servos[2], JOINT_MIN);
			servo_set(servos[1], JOINT_MIN);
			delayms(250);
		}
		else
		{
			servo_set(servos[2], 2100);
			servo_set(servos[1], 2000);
			delayms(250);
			servo_set(servos[2], JOINT_MAX);
			servo_set(servos[1], JOINT_MAX);
			delayms(250);
		}
	}
}

void flipOver(void)
{
	rollUp(0);
	
	servo_set(servos[2], JOINT_MID);
	delayms(250);
	orientation = (orientation + 1) % 2;
	
	lieFlat();
}

void inchWorm(unsigned char small)
{
	unsigned int pos1, pos2, delay;
	unsigned char i,j;
	
	if(small == 1)
	{
		pos1 = 1800;
		pos2 = 1000;
		delay = 250;
	}
	else
	{
		pos1 = JOINT_MAX;
		pos2 = JOINT_MIN;
		delay = 600;
	}
		
	for(i = 0; i < 6; i++)
	{
		for(j = 0; j < 3; j++)
			servo_set(servos[j],pos1);
		if(i == 0)
			delayms(delay/2);
		else
			delayms(delay);
		for(j = 0; j < 3; j++)
			servo_set(servos[j],pos2);
		delayms(delay);
	}
}		
